
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#include <math.h>

#define TAG "Main"
#define Turn_k 0.01

typedef struct {
  int16_t err_x;

} recv_data_t;

void parseUartData(recv_data_t *recv_data, uint8_t *serial_data, uint16_t len) {
  const uint8_t pack_head[2] = {0xa5, 0xa6};
  uint8_t *s = serial_data;

  for (uint16_t i = 0; i < len - 1; i++, s++) {
    if (*s == pack_head[0] && *(s + 1) == pack_head[1]) {
      break;
    }
  }

  if (s == serial_data + len - 1) {
    recv_data->err_x = 0;

    return;
  }

  s += 2;
  recv_data->err_x = ((*s == 1) ? 1 : -1) * (*(s + 1));
}

float delta_seconds = 0;

float get_delta_seconds() {
  static uint32_t last_ms = 0;
  uint32_t cur_ms;
  mspm0_get_clock_ms(&cur_ms);

  delta_seconds = (cur_ms - last_ms) / 1000.0f;
  last_ms = cur_ms;
  return delta_seconds;
}
int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();
  motion_init();
  log(TAG, "init_down");
  float delta_sec = 0;
  motion_set_speed(15);
  for (;;) {
    motion_move_pool(0.01);
    uint8_t *str = NULL;

    uint16_t str_len = getCurrentUart1Data(&str);
    if (str_len && str) {
      setCurrentUART1DataLock(true);

      recv_data_t recv_data;
      parseUartData(&recv_data, str, str_len);
      delta_sec = get_delta_seconds();

      motion_turn_pool(-recv_data.err_x, delta_sec, Turn_k);
      log(TAG, "err=%d\n", recv_data.err_x);
      setCurrentUART1DataLock(false);
    }
  }
}
